/*
 * Quat4f.cpp
 *
 *      Author: Philippe
 */

#include <cmath>

#include "Vect3f.h"
#include "Matrix4.h"
#include "Quat4f.h"


Quat4f::Quat4f() : w(1), v(.0, .0, .0) {}
Quat4f::Quat4f(float w, float x, float y, float z) : w(w), v(x,y,z) {}
Quat4f::Quat4f(float w, Vector3f<float> v) : w(w), v(v) {}

Quat4f::Quat4f(const Quat4f& q) {
    w = q.w;
    v = q.v;
}

Quat4f& Quat4f::operator=(const Quat4f& q) {
    w = q.w;
    v = q.v;
    return *this;
}
Quat4f Quat4f::operator+(const Quat4f& q) const {
    return Quat4f(w+q.w, v+q.v);
}
Quat4f Quat4f::operator-(const Quat4f& q) const {
    return Quat4f(w-q.w, v-q.v);
}

/*Quat4f Quat4f::operator/(const Quat4f& q) const {

}*/

Quat4f& Quat4f::operator/=(float scalar) {
    w /= scalar;
    v /= scalar;
    return *this;
}

Quat4f Quat4f::operator*(const Quat4f& q) const {
    return Quat4f(w*q.w - v.dot(q.v), q.v*w + v*q.w + (v^q.v));
}

float Quat4f::norm() const {
    return sqrt(w*w + v.dot(v));
}
float Quat4f::normSquared() const {
    return w*w + v.dot(v);
}

void Quat4f::normalize() {
    *this /= norm();
}

void Quat4f::conjugate() {
    v = v*(-1);
}

void Quat4f::inverse() {
    conjugate();
    *this /= normSquared();
}

Matrix4<float> Quat4f::matrix4() {
    if(norm() > 0.9999 && norm() < 1.0001) {
        return Matrix4<float>(
            Vect4<float>(1-2*(v.y*v.y+v.z*v.z), 2*(v.x*v.y+w*v.z), 2*(v.x*v.z-w*v.y), 0),
            Vect4<float>(2*(v.x*v.y-w*v.z), 1-2*(v.x*v.x+v.z*v.z), 2*(v.y*v.z+w*v.x), 0),
            Vect4<float>(2*(v.x*v.z+w*v.y), 2*(v.y*v.z-w*v.x), 1-2*(v.x*v.x+v.y*v.y), 0),
            Vect4<float>(0, 0, 0, 1));
    }
}

std::ostream& operator<<(std::ostream &o, const Quat4f &q) {
    o << "Quat4f("<< q.w << "," << q.v << ")";
    return o;
}
/*QDebug operator<<(QDebug qdbg, const Quat4f& q) {
    qdbg.nospace() << "Quat4f("<< q.w << "," << q.v <<")";
    return qdbg.space();
}*/

